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Abstract 

We present a complete characterization of shortest paths 
to a goal position for a vehicle with unicycle kinemat- 
ics and a limited range sensor, constantly keeping a given 
landmark in sight. Previous work on this subject stud- 
ied the optimal paths in case of a frontal, symmetrically 
limited Field-Of-View (FOV). In this paper we provide 
a generalization to the case of arbitrary FOVs, including 
the case that the direction of motion is not an axis of sym- 
metry for the FOV, and even that it is not contained in the 
FOV. The provided solution is of particular relevance to 
applications using side-scanning, such as e.g. in under- 
water sonar-based surveying and navigation. 

1 Introduction 

In several mobile robot applications, a vehicle with non- 
holonomic kinematics of the unicycle type, equipped with 
a limited range sensor systems, has to reach a target while 
keeping some environment landmark in sight. For exam- 
ple, in the Visual-Based control field the vehicle usually 
has an on-board monocular camera with limited Field- 
Of-View (FOV) and, subject to nonholonomic constraints 
on its motion, must move maintaining in sight one or 
more specified features of the environment. On the other 
hand, in the field of underwater surveying and naviga- 
tion, a common task for Autonomous Underwater Vehi- 
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cles (AUV) equipped with side sonar scanners is to de- 
tect and recognize objects (mines, wrecks or archeologi- 
cal find, etc.) on the sea bed (see e.g. [8j[l0)). Side-scan 
sonar is a category of sonar systems that is used to effi- 
ciently create an image of large areas of the sea. There- 
fore, in order to recognize objects AUVs must move keep- 
ing them inside the limited range of the sensor. 

Motivated by those application, in this paper we pro- 
pose the study of optimal (shortest) paths for a nonholo- 
nomic vehicle moving in a plane to reach a target position 
while making so that a given landmark fixed in the plane 
is kept inside a planar cone moving with the robot. 

The literature of optimal (shortest) paths stems mainly 
from the seminal work on unicycle vehicles with a 
bounded turning radius by Dubins [9|. Dubins has char- 
acterized the finite family of optimal paths for the particu- 
lar vehicle while a complete optimal control synthesis for 
this problem has been reported in J4]. Later on, a simi- 
lar problem with the car moving both forward and back- 
ward has been solved with different approaches in ifTTI . 
03). In particular, in 0141 the optimal control synthesis 
for the Reeds&Shepp car has been provided. Minimum 
wheel rotation paths in for differential-drive robots have 
been considered in J6). More recently, also the problem 
of determining minimum time trajectory has been taken 
into account in lfl6l . H) and Q for particular classes of 
robots, e.g. latter is on underwater robots. Finally, pre- 
vious works on the same subject of this paper ( iTPTl . lTT2l . 
0) have studied the optimal paths in case of a vehicle 
with a limited on-board camera but only with a symmetric 
FOV with respect to the forward direction of the robot. In 
this paper, we present a more general synthesis of shortest 
paths in case of side sensor systems, like side sonar scan- 
ners on UAVs, where the forward direction is not neces- 



sarily included inside the sensor range modeled as a cone 
centered on the vehicle. The impracticability of paths that 
point straight to the feature lead to a more complex anal- 
ysis of the reduction to a finite and sufficient family of 
optimal paths by excluding particular types of path. 

In the rest of the paper, we provide a complete opti- 
mal synthesis for the problem, i.e., a finite language of 
optimal control words (at most 15 words, depending on 
orientation of the sensor with respect to the forward direc- 
tion), and a global partition of the motion plane induced 
by shortest paths, such that a word in the optimal lan- 
guage is univocally associated to a region and completely 
describes the constrained shortest path from any starting 
point in that region to the goal point. 

2 Problem Definition 

Consider a vehicle moving on a plane where a right- 
handed reference frame (W) is defined with origin in Ow 
and axes X iv ,Z w . The configuration of the vehicle is de- 
scribed by §(f) = (*(*), z(f), 0(f)), where (x(t),z(t)) is 
the position in (W) of a reference point in the vehicle, 
and 6(t) is the vehicle heading with respect to the X w axis 
(see fig. [TJ. We assume that the dynamics of the vehicle 
are negligible, and that the forward and angular veloci- 
ties, v(f) and o(f) respectively, are the control inputs to 
the kinematic model. Choosing polar coordinates for the 
vehicle 77 = [p y//3] r (see fig. UK the kinematic model of 
the unicycle-like robot is 
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We consider vehicles with bounded velocities which can 
turn on the spot. In other words, we assume 



(v,co)eU, 



(2) 



with U a compact and convex subset of R 2 , containing 
the origin in its interior. 

The vehicle is equipped with a rigidly fixed sensor sys- 
tem with a reference frame (C) = {O c ,X c ,Y c ,Z c } such 
that the center O c corresponds to the robot's center 
[x(t ),z(t)] T and the forward sensor axis Z c forms an angle 
r w.r.t the robot's forward direction. Moreover, let 8 be 




Figure 1: Autonomous vehicle and systems coordinates. 
The vehicle's task is to reach P while keeping Ow within 
a limited sensor range modelled as a planar cone (high- 
lighted in color). 



the characteristic angle of the cone characterizing the lim- 
ited Sensor Range (SR) and let us consider the most inter- 
esting problem in which 8 < k/2. Without loss of gener- 
ality, we will consider < T < j, so that, when T = the 
Z c axis is aligned with the robot's forward direction (i.e., 
the particular case solved in lfT3l ). whereas, when r = § , 
is aligned with the axle direction. Consider <j)\ = T — £ 
and <j>2 — r+ f the angles between the robot's forward di- 
rection and the right or left sensor's border w.r.t. Z c axis, 
respectively. The restriction on < T = 



& < § will be 



removed at the end of this paper, and an easy procedure to 
obtain the subdivision for any value of F will be given. 

Without loss of generality, we consider the position of 
the robot target point P to lay on the Xw axis, with coordi- 
nates (p,\j/) = (pp, 0). We also assume that the feature to 
be kept within the SR is placed on the axis through the ori- 
gin Ow and perpendicular to the plane of motion. We con- 
sider a planar SR with characteristic angle 5 — \<j>2 — $i|> 
which generates the constraints 



J3-0i>O, 
j3-«fc<0. 



(3) 
(4) 



Note that we place no restrictions on the vertical di- 
mension of the sensor. Therefore, the height of the feature 
on the motion plane, which corresponds to its Y c coordi- 
nate in the sensor frame (C), is irrelevant to our problem. 



Hence, for our purposes, it is necessary to know only the 
projection of the feature on the motion plane, i.e., Ow- 

The goal of this paper is to determine, for any point Q G 
R 2 in the robot space, the shortest path from Q to P such 
that the feature is maintained in the SR. In other words, 
we want to minimize the length of the path covered by the 
center of the vehicle under the feasibility constraints (|T), 
©, ©, and ©. 

From the theory of optimal control with state and con- 
trol constraints (see O) it is possible to show that, when 
constraints (0 and dU are not active, extremals curves, 
i.e., curves that satisfy necessary conditions for optimal- 
ity, are straight lines (denoted by symbol S) and rotation 
on the spot (denoted by symbol *). On the other hand, 
when constraints © and (|4|i are active, the correspond- 
ing extremal maneuvers are two logarithmic spirals with 
characteristic angles fa and fa denoted by T\ and T2, re- 
spectively (see fPH for details). 

Logarithmic spiral T with characteristic angle <j) > 
(0 < 0) rotates counterclockwise (clockwise) around the 
feature. We refer to counterclockwise and clockwise spi- 
rals as Left and Right, and by symbols T L and T R , respec- 
tively. The adjectives "left" and "right" indicate the half- 
plane where the spiral starts for an on-board observer 
aiming at the feature. 

Notice that, for fa = n/2 the left sensor border is 
aligned with the axle direction and the spiral 7i becomes a 
circle centered in Ow (denoted by C), whereas for fa = 
the right sensor border is aligned with the direction of mo- 
tion and T\ becomes an half line through Ow (denoted by 
H). 

Extremal arcs can be executed by the vehicle in either 
forward or backward direction: we will hence use super- 
scripts + and — to make this explicit (e.g., S~ stands for 
a straight line executed backward). 

We will build extremal paths consisting of se- 
quences of symbols, or words, in the alphabet = 
{*, S + , S~, Et, E^}, where the actual meaning 

of symbols depends on angles T and 8 as in fig. [2] Rota- 
tions on the spot (*) have zero length, but may be used to 
properly connect other maneuvers. 

Let Jz?r be the set of possible words generated by the 
aforementioned symbols in srf for each value of Y. The 
rest of the paper is dedicated to showing that, due to the 
physical and geometrical constraints of the considered 
problem, a sufficient optimal finite language Jz?o C -S?r 




(a) Frontal: < T < § , E, = (b) Borderline Frontal: T = 
T, L ,E 2 = T«. %,Ei=H,E 2 = Tf. 
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(c) Side: § <T< %p,E 1 ■ 





(e) Lateral: ^ < T < f , 
Ei = 7f , E 2 = T 2 L . 



(f) Symmetric Lateral: T = 
^,Ei = TC,E 2 = T 2 L . 



Figure 2: Sensor configuration depending on angles T and 
8. 



can be built such that, for any initial condition, it contains 
a word describing a path to the goal which is no longer 
than any other feasible path. Correspondingly, a partition 
of the plane in a finite number of regions is described, for 
which the shortest path is one of the words in ££q- 



3 Shortest path synthesis 

In this section, we introduce the basic tools that will allow 
us to study the optimal synthesis of the whole state space 



of the robot, beginning from points on a particular sub- 
set of R 2 such that the optimal paths are in a sufficient 
optimal finite language. 

Definition 1. Given the target point P = (p/>, 0) in polar 
coordinates, andQ G R 2 \Ow, Q = (Pq, Wq) w ' tn PQ 0, 
let /q : R 2 — > R 2 denotes the map 

faiPcWc) = < V Pq J (5) 

I (0,0) otherwise. 

The map /g is the combination of a clockwise rotation 
by angle y/c — Wq> an d a scaling by a factor Pp/Pq that 
maps Q in P. 

Remark 1. The alphabet stf is invariant w.r.t. rota- 
tion and scaling. However, it is not invariant w.r.t. 
axial symmetry, as it happened in the particular case 
(i.e., the Frontal case with T = 0) considered in M3V , 
where the map fg was defined as a combination of ro- 
tation, scaling and axial symmetry. For example, loga- 
rithmic spirals are self-similar and self-congruent ( under 
scaling and rotation they are mapped into themselves). 
On the other hand, left (right) spirals are mapped into 
right (left) spirals through an axial symmetry and alpha- 
bet invariancy can be lost. Indeed, for example, con- 
sidering the Side case alphabet ( see fig. \2(c)\ J^side = 
{*, S + , S~ , , , T^}, and applying an axial 

symmetry we have Tf — > ^ £?side> the same occurs for 
the Frontal alphabet with T > 0. 

Let 7 be a path parameterized by t G [0, 1] in the plane 
of motion y(t) = (p(f), Denote with &>q the set of 

all feasible extremal paths from y(0) = Q to 7(1) = P. 

Definition 2. Given the target point P = (p/>, 0) and Q = 
(Pq, \j/g) with Pq 7^ 0, let the path transform function Fq 
be defined as 

y(t)^f Q (y(i-t)),Vtei. 

Notice that 7(f) = Fg(y(l— t)) corresponds to 7(f) 
transformed by fg and followed in opposite direction. In- 
deed, 7 is a path from 7(0) = /q(P) = (j^, -Ve) t0 
f(l)=f Q (Q)=P. 



The F map has some properties that make it very use- 
ful to the study of our problem in a way which is to some 
extent similar to what described (for a different F map) 
in |[T3l . In particular, the locus of points Q such that 
/q(P) = Q, is the circle with center in Ow and radius pp. 
We will denote this circle by C{P) and the closed disk 
within C(P) by D{P). 

C(P) has an important role in the proposed approach 
since properties of Fq will allow us to solve the synthesis 
problem from points on C(P), and hence to extend the 
synthesis to D(P) and to the whole motion plane. Indeed, 
Vg G C(P) and V 7 G &q, Fq{j) G 9 fQ{P) with f Q {P) G 
C(P), i.e., a path from a point on C(P) to P is mapped in 
a path from C{P) to P. 

Furthermore, Fq transforms an extremal in si in itself 
but followed in opposite direction. Hence, Fq maps ex- 
tremal paths in «5fp in extremal paths in Jzfp- For example, 
let w = S~ * H * S + * 7j be the word that character- 
ize a path from Q to P, the transformed path is of type 
z = * S~ * H + * S + . With a slight abuse of notation, 
we will write z = Fq(w). 

Proposition 1. Given Q G R 2 and a path 7 G 2?q of 
length I, the length of the transformed path y ' — Fq(y) is 

1 = 

pq 

The proof is easily obtained from a similar result 
inJH. 

Based on the properties of Fq, optimal paths from 
points on C(P) completely evolve inside C(P). To prove 
this statement we first report the following result, 

Theorem 1. Given two points A = (pa, Ya) and B = 
(Pb, Wb), with > y/fl and p = Pa = Pb, and an ex- 
tremal path yfrom A to B such that for each point G of y, 
Pq > p, there exists an extremal path yfrom A to B such 
that for each point G of y p G < p and £(y) < £(y) (see 
fig.®. 

The proof of this theorem can be found in section [Tlin 
the Appendix. 

An important but straightforward consequence of the 
theorem is the following 

Corollary 1. For any path in &q with Q G C(P) there ex- 
ists a shorter or equal-length path in £?q that completely 
evolves in D(P). 



Figure 4: Forward and backward straight path regions from G for § <T< 




Figure 3: An example for theorem Q] path 7 = 7172 (71 
followed by 72) of type 7^ _ S~ * from A to B is short- 
ened by a path 7 = 71 72 of type T\ * T^ + S~ by applying 
path transformation /"z to path 7. 

4 Optimal paths for points on C(P) 

Our study of the optimal synthesis begins in this section 
addressing optimal paths from points on C(P). We first 
need to establish an existence result of optimal paths. 

Proposition 2. For any Q S C(P) there exists a feasible 
shortest path to P. 

Proof. Because of state constraints (fjjl, and (0), and the 
restriction of optimal paths in D(P) (Corollary[T|i the state 
set is compact. Furthermore, it is possible to give an 
upper-bound on the optimal path length for all F G [0, j]. 
Indeed, given a point Q at distance p from Ow the optimal 



path to P is shorter or equal to the following paths based 
on the value of F and 5: 

• Frontal (0 < F < §): S + * S~ or H+ * H of length 
P+Pp\ 

• Side (f < T < S^L): 7f + * T^~, of length 
I Pp ^ + Pp I 1 n I , where N is the intersection point 

\ COS 0[ COS(/>2 J v 

between spirals Tyg and Tfp through Q and P respec- 
tively; 

• Borderline Side (F = 7f + * C P ) of length 
(^i^f + _ Wp)Pp^J > where N is the intersection 
point between spirals 7j and Cp\ 

• Lateral < F < § ): T^ Q * T* p , of length 
( cos^J + ^col^f ) ' w here N is the intersection point 
between spirals T^q and Tf p . 

The system is also controllable because there always ex- 
ists an intersection point between two spirals (even if de- 
generated in half-lines or circumferences) with different 
characteristic angle even if both clockwise or counter- 
clockwise around the feature. Hence, Filippov existence 
theorem for Lagrange problems can be invoked [5 1. □ 

In the following we provide a set of propositions that 
completely describe a sufficient optimal finite language 
for all values of re [0, §]. 




Figure 5: Forward and backward straight path Regions 
from G for < T < f. 



Definition 3. For any starting point G = (pc, Wg)> l et 
SF(G) (SB(G)) be the set of all points reachable from G 
with a forward (backward) straight line without violating 
the SR constraints. 

We denote with dSF x (G) and dSF 2 (G) (dSB\(G) and 
dSB 2 {G)) the borders of SF(G) (SB(G)). Also, let C,(G) 
denote the circular arcs from G to Ow such that, W G 
Ci{G),G~VCh? = n-\<t>i\. 

Remark 2. Based on simple geometric considerations, 
for any starting point G = (p^, Xj/c), for < T < 4 
(Frontal Case), SF(G) is the region between dSF 2 (G) = 
C2(G) and dSF\(G) = C\{G). Let r\(G) (r 2 (G)) denote 
the half-line from G forming an angle xj/q — <j>i ( \f/tj — tyi) 
with the Xw axis (cf. fig. \5§. SB(G) is the cone delimited 
by dSB\ (G) = r\ (G) and dSB2(G) = r 2 (G), outside cir- 
cle with center in Ow and radius Pg- Notice that, SF(G) 
lays completely in the circle with center in Ow and ra- 
dins Pq. Moreover, in the particular case in which T = ^ 
(Borderline Frontal Case), E\ — H and dSF\ (G) degener- 
ates in the chord ( GOw ) between G and Ow, aligned with 

n(G). 




Figure 6: Forward and backward straight path Regions 
from G for ^ <T< f. 



arc between G and Gf, SF(G) is the region between arc 
dSF 2 (G) ~Cg f and chord dSF\{G) = Sg f . Consider the 
rotation and scale that maps Gf in G and G in Gb- we 
have dSBiiG) = dSFi(G B ), i.e. dSB 2 {G) = dSF 2 {G B ). 
Moreover, for all point V on the circular arc Cq b from Gb 

to G, angle GgVOw — Tl—\<fo\, and angle OwGgG = <j>\. 
Notice that, in this case, SF(G) lays completely in the cir- 
cle with center in Ow and radius Pq. Notice that, in the 
particular case in which Y = (Borderline Side Case), 
E 2 = C and dSF 2 (G) is an arc from G to Gf on a semi- 
circle with diameter Pq. 



As a consequence of Remark[3] SF(G) is tangent in G 
to Tf and or C. Moreover, SF(G) is tangent in Gf to 
7f and T 2 R or C, see fig.ED 

Fig. [6] shows the SF(G) and SB(G) regions described 
in|3]for the Lateral case. Notice that, in this case, SF(G) 
does not lay completely in the circle with center in Ow 
and radius Pq. 

Remark 4. Optimal forward (backward) straight arcs 
and G F = (pG^fe, Wg + (02 - 01 )) G C 2 (G), i.e. such f rom an y G ends on C G F (C Gb ) (see also ^Hj for details), 
that OwGGf = 01 (cf fig. @). Naming with Cq f the Based on all the above properties, we are now able to 



As a consequence of Remark|2] both SF(G) and SB(G) 
are tangent in G to or H and 

Remark 3. For any starting point G = (pc, Wg)> and for 

^ < Y < ^—S /VIA* ns,eo\ lot C„ ha tho r-hnvrl UoUA,oon C, 

'sinfc< Vg + (02-00) GC 2 (G), i.e. 



obtain a sufficient family of optimal paths by excluding 
particular sequences of extremals. 

Theorem 2. Any path consisting in a sequence of a back- 
ward extremal arc followed by a forward extremal arc is 
not optimal. 

The proof of this theorem, whose details can be found 
in section [2] of the Appendix, is based on the fact that 
for continuity of paths, for any sequence of a backward 
extremal followed by a forward one, there exist points A 
and B that verify hypothesis of TheoremQ] 

Theorem 3. Any path consisting in a sequence of an ex- 
tremal arc Ei and an extremal arc E followed in the same 
direction is not optimal for any i,j £ {1,2} with i =/= j. 

Notice that the feasible sequences consisting of two 
extremals that we still need to discuss are those starting 
or ending with S followed in any direction (E + E~ and 
E~E + are obviously not optimal). 

Proposition 3. From any starting point A, any path y of 
type S + * E^ and 5 + * £j~ to B can be shortened by a path 
of type S + E2 or E^ *E^. Moreover, any path 7 of type 
S + * £[ + or S + * E2 can be shortened by a path of type 
E+S + orE+ *E 2 ~. 

Proposition [3] implies that paths of type S~ * Ej~ and 
S~ * E2 are not optimal. Indeed, they can be shortened 
by S~E^ and E^S^, respectively (see fig. |7]for the Side 
case). 

By using all previous results, a sufficient family of opti- 
mal paths is obtained in the following important theorem. 

Theorem 4. For 4 < T < f , i.e. Side and Lateral cases, 
and for any Q £ D(P) to P there exists a shortest path of 
type E^ *E2~S~E^ or of type E^S+E^ *£f . ForO < T < 
j, i.e. Frontal case, and for any Q £ D(P) to P there exists 
a shortest path of type S + E^ ^E^S^ or of type 5 + £' 2 h * 
E-S-. 

Proof. According to all propositions above several con- 
catenations of extremal have been proved to be non op- 
timal. Considering extremals as node and, possibly opti- 
mal, concatenations of extremal as edges of a graph, the 
sufficient optimal languages Jzfp from Q in D(P), for dif- 
ferent values of r and <S, are described in fig.0 Indeed, it 
is straightforward to observe that the number of switches 




a) b) 



Figure 8: Feasible extremals and sequence of extremals 
from point in D(P): a) in Side and Lateral cases (4 < T < 
f ). b) in Frontal case (0 < T < f ). 

between extremals is finite and less or equal to 3, for any 
value of r and 8. Hence, the thesis. □ 

We now study the length of extremal paths from C(P) 
to P in the sufficient family above. 

Without loss of generality, it is sufficient to study the 
length of extremal paths of type E^ * E^S^E^ only from 
points Q on the semicircle of C(P) in the upper-half plane 
(denoted by CS). Indeed, up to a rotation, optimal paths 
of type E^S + E2 * £f from the rest of C(P) can be easily 
obtained. Referring to fig. [9] let the switching points of 
the optimal path be denoted by N, M\ and M 2 or N, M\ 
and M2 = P, respectively, depending on the angular values 
(Xmi or a,M { . Moreover, in order to do the analysis, it is 
useful to parameterize the family by the angular value 
of the switching point M\ along the arc C2 (P) between P 
and Z or the angular value a*^ of the switching point M\ 
along the extremal E\ between Pf and Ow- 

Theorem 5. For any point Q £ CS, the length of a path 
Y£^ Q of type * E^S-E^ is: 

• for < OCtf < <j>2 ~ 01> i- e - from P to Z (notice that 
the last arc has zero length): 

( cos a Mi 1 
L = pp< — '- H — + 

COS 02 COS 01 

l\ 

_ cos 0i +COS02 c ( Ve -q Mi )im- f sin((fe-« Ml ) \ < 2 -<i 
cos 0i cos 02 \ sin 02 / 

(7) 



(a) From A to B, path S + * T 2 R+ through z and v can (b) From A to B' , path S + * through z can be short- 
be shortened by S + T 2 R+ through v, where 5 arc is ened by a path of type S + T 2 R+ through v, whereas from 
tangent to Tf. A to B" by a path of type T 2 R+ * T*~ through g. 




(c) From A to B, path S + *T R+ through z can (d) From A to B 1 , path S + * T 2 through z can be shortened 
be shortened by a path of type T R+ S + through v, by a path of type T R+ S + through v, whereas from A to B" 
where S arc is tangent to 7f . by a path of type T R+ * T 2 R " through g. 



Figure 7: Examples of paths shortened in proposition[3]for the Side case. 



• for > 02 — 0i, i.e. from Z to Ow-' 



L = p P 



2 



COS 01 



COS(02 — 01 ) 1 
COS 02 COS 01 



COS$ 
COS 



'1 + COS 02 c [y fl -(fe-0i)] ffi ( sin 0i V 
01 cos 02 \ sin 02 / 



(8) 



with t\ = 1/ tan 0i and t-i = \j tan 2 . 



The analytical expression for the length L is based on 
a direct computation. Having the path's length as a func- 
tion of two parameters olm v or and \]/q, we are now 
in a position to minimize the length within the sufficient 
family. 



Theorem 6. Given a point Q £ CS, 

• forO< Wo < ufe, := sin <fe-^> In f . co ;^+ c ° s ^ , 
optimal path is of type * E^; 



for y Rl <\]/q< y/R 2 with y/ fl2 := (0 2 - 0i) + \I/ Rl 



tan 02 In 



sin 0i 
sin (j)2 



optimal path is of type E^ *E~, S 



• for \f/ R ., < y/g < n the optimal path is £j * E^ 
through Ow- 

Moreover, for i//g = y/ Rl , any optimal path of type E^ * 
£ , -2~5 _ £']~ turns out to have the same length I of optimal 
path Ej *£f . Hence, for \]/q = \]/r 2 a/so E^ ^E^S'E^ 
is optimal. 




Figure 9: Path of type E\ 



*E 2 S~ 



E l or the degenerate 



case of type £j *E 7 S from Q £ CS. 



Previous results have been obtained computing first and 
second derivatives of L and nonlinear minimization tech- 
niques. 

We are now interested in determining the locus of 
switching points between extremals in optimal paths. 

Proposition 4. For Q e CS with < l/g < VRi' 
the switching locus is the arc of E2 between P 



M = (p P 



tan 02 In ( ^ 



sin02 sin(02— </>i! 



COS <j)[ +COS 02 



j Wm) (included), where xj/m 



Proof. From Theorem [6] the optimal path from Q £ CS 
to P is of type E^ *E 2 . For \^q — 



between £j and E 2 



is M. 



the intersection 
□ 



Proposition 5. For Q G CS with i^r, < xyg < y/ ff2 , the 
loci of switching points Mj_ and N are the dSF2(P) and 
dSF 2 (M). 

Proof. For Q £ CS with y/R l < y/g < y/R 7 , considering the 
values of (Xm 2 obtained in the computations of Theorem[6] 
we obtain M2 G dSF2(P). Furthermore, substituting those 
values in the equation of the intersection point between 
£1 through Q and £2 throughMo we obtain G dSF2(M). 

□ 

Finally, for Q G CS with \j/ Rl < < n, the switching 
locus reduces to the origin Ow since two extremal £; in- 
tersect only in the origin for ; = 1,2. 
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Figure 10: Optimal synthesis inside D(P). 

5 Shortest paths from any point in 
the motion plane 

The synthesis on C(P) induce a partition in regions of 
D(P). Indeed, for any Q G D(P), there exists a point 
V &C(P) such that the optimal path 7 from V to P goes 
through Q. The Bellmann's optimality principle ensure 
the optimality of the sub-path from Q to P. Based on this 
construction the partition of C(P) is reported in fig.fTQl 

For points outside C(P), function Fq has been defined 
in [6] in order to transform paths starting from Q inside 

C(P) in paths starting from /g(P) = m^, — Woj outside 

C(P). 

From other properties of Fq, such as Proposition Q] we 
have also that an optimal path is mapped into an optimal 
path. Hence, the optimal synthesis from points outside 
C(P) can be easily obtained mapping through map Fq all 
borders of regions inside C(P). 

Proposition 6. Given a border B and Q G B map Fq 

transforms: 



1. B 



2. B 



3. B 



: C(P) into itself; 

■ dSF 2 (Q)indSB 1 {f Q (P)) 

■dSF l {Q)indSB 2 (f Q {P)) 



Figure 1 1 : Partition of the motion plane for f < T < 



Figure 12: Partition of the motion plane for r = 8/2 (i.e. 
a SR border is aligned with the robot motion direction, 
Borderline Frontal). 



4. B = Ej in arcs of the same type (i= 1 , 2) 

Proof. The proof of this proposition can be found in lTT3l . 

□ 

Based on Proposition [6] the optimal synthesis of the 
entire motion plane is reported in fig. [TT] 



6 Optimal synthesis for generic r 

We first obtain the synthesis of the Borderline Frontal 
case, i.e. r = f , reported in fig. [T2lfrom the one obtained 
in the previous section. 

Notice that, E\ = Tf of the Side case degenerates in a 
straight line H through Ow for T = % . Indeed, referring 
to fig.njj points Mf and Pp degenerate on Ow- As a con- 
sequence, Region IV, IV and VI 1 while coordinates x ¥r 1 
and ^Prj of points R\ and R2 can be obtained from values 
in|6]replacing ty\ = 0. 

In the Frontal case, E\ = H becomes a spiral 
Ty, straight lines from P and R2 split in straight 
line and a spiral arc generating the partition re- 
ported in fig. Qj] In this case, (pi < and points 
Ri and ^2 do not lay on C(P) but on a circle 

through P with center (0, — Dp ~, sm /'- I'" ), where £ = 




Figure 13: Partition of the motion plane for < T < 4, 
i.e. Frontal case. 



■ In (— sin 0i ) — j- In (sin $2). No- 



?l+?2 j n / COS0l+COSfe 
(1/2 sin0 2 -<|>i) J ' t\ 

tice that for <$h = —<j>u this circle coincide with C(P) and 
the synthesis proposed in ifTJl is obtained. 

Referring again to fig. QJJ in the Borderline Side case 
(r = i.e. the SR border is aligned with the axle 
direction and $2 = f ), E% = T 2 R degenerates in = C. 
Points Ri=M and R 2 lays on C(P) with m Rl = 
and = -5 — 0i + v Fr i + tan0i ln(sin^i). The obtained 




Figure 14: Partition of the motion plane for F = (i.e. 
a SR border is aligned with the axle direction). 




Figure 15: Partition of the motion plane for ^jl < r < § 
(i.e. axle direction is included inside the SR). 

synthesis is reported in fig. [14] For the Lateral case E2 = C 
becomes E% = T 2 and the synthesis of the Lateral case, re- 
ported in fig. Q3] can be obtained from the one in fig. [14] 

The subdivision of the motion plane in case of ^ < F < 
K can be easy obtained by using that one for < F < % 
considering optimal path followed in reverse order, i.e. 
forward arc in backward arc and viceversa. Finally, a 



symmetry w.r.t. Xw axis of each subdivision of the motion 
plane for each F € [0, n] allows to obtain the correspond- 
ing subdivision for T G [— n, 0]. 

7 Conclusions and future work 

A complete characterization of shortest paths for unicy- 
cle nonholonomic mobile robots equipped with a limited 
range side sensor systems has been proposed. A finite suf- 
ficient family of optimal paths has been determined based 
on geometrical properties of the considered problem. Fi- 
nally, a complete shortest path synthesis to reach a point 
keeping a feature in sight has been provided. A possible 
extension of this work is to consider a bounded 3D SR 
pointing to any direction with respect to the direction of 
motion. A more challenging extension would be consid- 
ering a different minimization problem such as the mini- 
mum time. 
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. 1 Proof of Theorem [I] 

Theorem 1. Given two points A = (p^, xj/^) and B = 
(ps, Wb), with Ya > Wb and p = Pa = Pb, and an ex- 
tremal path y from A to B such that for each point G of y, 
Pq > p, there exists an extremal path f from A to B such 
that for each point G of ' f, pg < p and 1(f) < l(y). 

Proof. Consider a point Z = (pz, l/z) such that pz = 
maxoey Pg > P- Let 71 and 72 the sub-paths of 7 from 
Z to B and from Z to A. 

The sub-path 71, is rotated and scaled (contracted of 
factor pj- < 1) such that Z is transformed in A obtaining 

2 

a path fx from A to Z = y/ A + \f/ B - Vz)- Similarly, 
72, can be rotated and scaled with the same scale factor but 
different rotation angle w.r.t. Yi such that Z is transformed 
in B, see fig- [3] After geometrical considerations, it is easy 
to notice that the obtained path fi starts in B and ends in 
Z. 

The obtained paths are a contraction of 71 and 72 re- 
spectively and hence shorter. Moreover, any point G of 
71 or j2 has pc > p hence is scaled in G of fl or 71 with 

Pg = ^<P- 

Concluding, we have obtained a shorter path from A to 

B that evolves completely in the disk of radius p. □ 

.2 Proof of Theorem |2] 

Theorem 2. Any path consisting in a sequence of a back- 
ward extremal arc followed by a forward extremal arc is 
not optimal. 

Proof. Observe that the distance from Ow is strictly in- 
creasing along backward extremal arcs (i.e. S~, £f , EI 
with £2 7^ O and strictly decreasing along forward ex- 
tremal arcs (i.e. S + , E^, E^ with £2 7^ C). For continuity 
of paths, for any sequence of a backward extremal fol- 
lowed by a forward one, there exist points A and B that 
verify hypothesis of TheoremQ] hence it is not optimal. 

Any sequence consisting in an extremal S (or E\) of 
length £ and an extremal E^=C (in any order and direc- 
tion) is inscribed in two circumferences centered in Ow- 
Hence, the shortest sequence is the one with E^=C along 



the circle of smaller radius necessarily preceded by a for- 
ward S (or Ei) of same length I. 

Concluding, in an optimal path a forward arc cannot 
follow a backward arc. □ 



